import edu.wsu.KheperaSimulator.RobotController;

/**
 * Subsumption main class
 * @author Markus Wetzel & Philipp Rusch
 *
 */
public class Subsumption extends RobotController {

	private final StuckController stuckController = new StuckController(this);
	private final PushController pushController = new PushController(this);
	private final GoToNextBallController goToNextBallController = new GoToNextBallController(
			this);
	private final SearchController searchController = new SearchController(this);

	@Override
	public void close() throws Exception {
	}

	@Override
	public void doWork() throws Exception {

		// falls kein Controller anspricht fahren wir gerade aus
		setMotorSpeeds(5, 5);

		// stecken geblieben?
		if (stuckController.isStuck()) {
			stuckController.doWork();
		} else {
			// schieben?
			if (pushController.isPushing()) {
				pushController.doWork();
			} else {
				// zum ball gehen?
				if (goToNextBallController.isGoingToNextBall()) {
					goToNextBallController.doWork();
				} else {
					// ball suchen
					searchController.doWork();
				}
			}
		}
	}

}
